/***************************************************************
 * The entrance of ROS communication and stanley control
 * @author Huyunhao
 * @date 2020-12-30
 * */
#include <ros/ros.h>
#include "ros_stanley/stanley.h"  //ros msg header
#include "stanley_controller.h"

// define a global variable to receive subscribe data.
ros_stanley::stanley msg_rx;    //stanley msg receive;
ros_stanley::stanley msg_tx;    //stanley msg transfer;

void chatterCallback(const ros_stanley::stanleyConstPtr &msg) {
    ROS_INFO("I receive [x,y,yaw,v]=[%f,%f,%f,%f]", msg->x, msg->y, msg->yaw, msg->v);
    msg_rx = *msg;
}

void runRos(int argc, char **argv) {
    // Initiate ROS,and give the node name
    ros::init(argc, argv, "n_stanley_controller");
    // create a handle for the node,which initializes the node.
    ros::NodeHandle n;
    // topic you want to publish
    ros::Publisher publisher = n.advertise<ros_stanley::stanley>("control", 1000);
    // topic you want to subscribe
    ros::Subscriber subscriber = n.subscribe("preScan", 10000, chatterCallback);
    // the publisher cycle time
    ros::Rate loop_rate(100);
    //***************************Initialize Stanley Controller parameter*********************
    // 1.初始化车辆状态
    controlNS::State state(0,0,0,0);
    controlNS::StanleyController stanleyController;
    // Enter the ROS cycle
    while (ros::ok()) {
        ros::spinOnce();
        float delta = stanleyController.calcDelta(state);   //delta的单位是[rad]
        msg_tx.delta = myNumpy::rad2deg(delta) * controlNS::kSteeringRatio; //车轮转角*传动比
        msg_tx.error_front_axle = stanleyController.cross_track_error_;
        ROS_INFO("I send delta[%f]", msg_tx.delta);
        state.UpdateForPreScan(msg_rx.x, msg_rx.y, msg_rx.yaw, msg_rx.v);   //更新车辆的状态
        publisher.publish(msg_tx);
        loop_rate.sleep();
    }

}

void test01() {

}

int main(int argc, char **argv) {
    runRos(argc,argv);
    // test01();
    return 0;
}
